package msg

import "math"

type LaserScan struct {
	Header         Header    `rosbag:"header"`
	AngleMin       float32   `rosbag:"angle_min"`
	AngleMax       float32   `rosbag:"angle_max"`
	AngleIncrement float32   `rosbag:"angle_increment"`
	TimeIncrement  float32   `rosbag:"time_increment"`
	ScanTime       float32   `rosbag:"scan_time"`
	RangeMin       float32   `rosbag:"range_min"`
	RangeMax       float32   `rosbag:"range_max"`
	Ranges         []float32 `rosbag:"ranges"`
	Intensities    []float32 `rosbag:"intensities"`
}

func SerializeLaserScan(funcData *FucData) {
	var data LaserScan
	err := funcData.Record.ViewAs(&data)
	if err != nil {
		funcData.ErrorCallback(funcData.Topic, err)
		return
	}
	newData := data

	for i := 0; i < len(data.Ranges); i++ {
		if math.IsNaN(float64(data.Ranges[i])) {
			newData.Ranges[i] = 0
		} else {
			newData.Ranges[i] = data.Ranges[i]
		}
	}
	for i := 0; i < len(data.Intensities); i++ {
		if math.IsNaN(float64(data.Intensities[i])) {
			newData.Intensities[i] = 0
		} else {
			newData.Intensities[i] = data.Intensities[i]
		}
	}

	funcData.Callback(TopicData{
		Topic:   funcData.Topic,
		MsgType: funcData.Record.ConnectionHeader().Type,
		Data:    newData,
	})
}
